# -*- coding: UTF-8 -*-
import os
import yaml
import rospy
#from module import IMU_Synchonizer
from module import Image_Synchronizer
from module import IMU_Denoise

imu_topic=None
imu_pub=None
image_topic=None
image_pub=None
imu_rate=None
img_rate=None

def initParameter():
    global imu_topic, image_topic, imu_pub, image_pub,imu_rate,img_rate
    fs = open(os.path.join("./config", "parameter.yaml"), encoding="UTF-8")
    datas = yaml.load(fs, Loader=yaml.FullLoader)
    imu_topic = datas['imu_topic']
    image_topic = datas["image_topic"]
    imu_pub = datas["imu_pub"]
    image_pub = datas["image_pub"]
    imu_rate=datas["imu_rate"]
    img_rate=datas["image_rate"]
    print("parameters init done")


if __name__ == "__main__":
    rospy.init_node('FeatureProject',anonymous=True)
    initParameter()
    imudenoiser=IMU_Denoise.IMU_Denoiser(imu_topic,imu_pub,"","","")
    imudenoiser.start()
    ImageTask=Image_Synchronizer.Image_Synchronizer(image_topic,image_pub,img_rate)
    ImageTask.start()
    rospy.spin()
